/*
 * @Description: 点云畸变补偿
 * @Author: Zhijian Qiao
 * @Date: 2020-02-25 14:38:12
 */

#ifndef avp_slam_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_
#define avp_slam_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_

#include <pcl/common/transforms.h>
#include <Eigen/Dense>
#include "glog/logging.h"

#include "models/scan_adjust/distortion_adjust.hpp"
#include "sensor_data/velocity_data.hpp"
#include "sensor_data/cloud_data.hpp"

namespace avp_slam {
    class DistortionAdjust {
    public:
        void SetMotionInfo(float scan_period, VelocityData velocity_data);

        bool AdjustCloud(CloudRingData::CLOUD_PTR &input_cloud_ptr, CloudRingData::CLOUD_PTR &output_cloud_ptr);

        bool AdjustCloud(CloudRingData::CLOUD_PTR &input_cloud_ptr, CloudRingData::CLOUD_PTR &output_cloud_ptr,
                         Eigen::Matrix4f T_adjust);

    private:
        inline Eigen::Matrix3f UpdateMatrix(float real_time);

    private:
        float scan_period_;
        Eigen::Vector3f velocity_;
        Eigen::Vector3f angular_rate_;
    };
} // namespace lidar_slam
#endif